#include <string>
#include "xbox.h"
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <thread>
#include <mutex>

static xbox_map g_dataMap;
std::mutex m;

void reciveThread()
{

    ros::NodeHandle nh("~");
    std::string device;
    nh.param<std::string>("device", device, "/dev/input/js0");
    XBoxControl xbox(device);

    while (ros::ok())
    {
        xbox_map data;
        if (xbox.readData(data))
        {
            std::lock_guard<std::mutex> lg(m);
            g_dataMap = data;
        }
    }
}

int main(int argc, char **argv)
{
    // initialize our ROS node, create a teleop_turtle, and spin our node until Ctrl-C is pressed
    ros::init(argc, argv, "xbox_control");
    ros::NodeHandle nh("~");
    ROS_INFO("init");

    float driftRatio;
    nh.param<float>("drift", driftRatio, 0.1);

    float frequency;
    nh.param<float>("rate", frequency, 30);

    bool useAxisY;
    nh.param<bool>("useAxisY", useAxisY, false);

    std::string topicName;
    nh.param<std::string>("topicName", topicName, "/cmd_vel");

    float linearMax = 0.5, angularMax = 1;
    ROS_INFO("line max: %f angular max: %f", linearMax, angularMax);

    ROS_INFO("success");

    auto velPub = nh.advertise<geometry_msgs::Twist>(topicName, 1);
    geometry_msgs::Twist twist;
    twist.angular.x = twist.angular.y = twist.angular.z = 0;
    twist.linear.x = twist.linear.y = twist.linear.z = 0;
    geometry_msgs::Twist stopTwist = twist;

    std::thread t(reciveThread);

    ros::Rate rate(frequency);
    bool move = false;
    xbox_map dataMap;
    while (ros::ok())
    {
        {
            std::lock_guard<std::mutex> lg(m);
            dataMap = g_dataMap;
        }

        if (dataMap.xx < 0)
            angularMax -= angularMax * 0.01;
        if (dataMap.xx > 0)
            angularMax += angularMax * 0.01;

        if (dataMap.yy < 0)
            linearMax += linearMax * 0.01;
        if (dataMap.yy > 0)
            linearMax -= linearMax * 0.01;

        if (dataMap.xx != 0 || dataMap.yy != 0)
        {
            ROS_INFO("line max: %f angular max: %f", linearMax, angularMax);
        }

        if (dataMap.a)
            move = true;
        if (dataMap.b) // stop
            move = false;

        // update twist'
        if (useAxisY)
        {
            if ((float)abs(dataMap.ly) > XBOX_AXIS_VAL_MAX * driftRatio)
                twist.linear.y = -(float)dataMap.ly / XBOX_AXIS_VAL_MAX * linearMax;
            else
                twist.linear.y = 0;
        }
        else
        {
            if ((float)abs(dataMap.ly) > XBOX_AXIS_VAL_MAX * driftRatio)
                twist.linear.x = -(float)dataMap.ly / XBOX_AXIS_VAL_MAX * linearMax;
            else
                twist.linear.x = 0;
        }

        if ((float)abs(dataMap.rx) > XBOX_AXIS_VAL_MAX * driftRatio)
            twist.angular.z = -(float)dataMap.rx / XBOX_AXIS_VAL_MAX * angularMax;
        else
            twist.angular.z = 0;

        if (move)
            velPub.publish(twist);
        else
            velPub.publish(stopTwist);

        rate.sleep();
    }

    t.join();
}